#include	"bsp_stepper_T_speed.h"
#include "bsp_stepper_init.h"
#include	"bsp_sensor.h"
#include	"math.h"
//函数以及变量声明
extern TIM_HandleTypeDef htim1;
step_motor_t step_motor_axis[4];


/************************注意***********************
	实测触发的最小定时器间隔12;因此需要对最大速度进行限制
	最大速度
	15707 也就是1570.7rad/s
**************************************************/



/**
* @brief    电机回零
* @author   SiYu Chen
* @param[in] step_motor  电机结构体
* @param[in] accel  加速度,如果取值为10，实际值为10*0.1*rad/sec^2=1rad/sec^2
* @param[in] decel  减速度,如果取值为10，实际值为10*0.1*rad/sec^2=1rad/sec^2
* @param[in] speed  最大速度,如果取值为10，实际值为10*0.1*rad/sec=1rad/sec
* @param[in] time_out  最大等待时间
* @retval -1  超时   
* @retval -2  超最大步数  
* @retval 1   成功  
 */
int8_t motorHoming(step_motor_t *step_motor,uint32_t accel, uint32_t decel, uint32_t speedfast,uint32_t speedlow,uint32_t time_out)
{
	
		uint32_t start_time = HAL_GetTick(); //获取当前时间
	  _Bool SOFT_LIMIT_CONFIG = step_motor->ENABLE_HARD_LIMIT;//记录软件限位设置
		step_motor->ENABLE_HARD_LIMIT=0; //关闭软件限位
		if(step_motor->HOME_DIR==CW) 
		{
			 /******************************快速运动到零点位置***********************************/
			 stepperMove(step_motor,step_motor->MAX_MOVE_STEP,accel,decel,speedfast);	
			 while(HAL_GPIO_ReadPin(step_motor->SENSOR_CW_PORT,step_motor->SENSOR_CW_PIN)!=0)
			 {
					if(HAL_GetTick()-start_time>=time_out)
					{
							step_motor->ENABLE_HARD_LIMIT=SOFT_LIMIT_CONFIG; 
						  stopStepMotor(step_motor);
							return -1;//超时
					}
					if(	step_motor->status.running == FALSE) //运行结束
					{
						step_motor->ENABLE_HARD_LIMIT=SOFT_LIMIT_CONFIG; 
							return -2;//超最大步数
					}
			 }
			 stopStepMotor(step_motor);
			 HAL_Delay(500);
			 //setStepPosition(step_motor,0);//零点	 
			 /******************************切换运动方向慢速运行离开原点位置***********************************/
			 stepperMoveWithSpeed(step_motor,-speedlow,step_motor->MAX_MOVE_STEP);
	
			 
			 while(HAL_GPIO_ReadPin(step_motor->SENSOR_CW_PORT,step_motor->SENSOR_CW_PIN)==0)
			 {
					if(HAL_GetTick()-start_time>=time_out)
					{
							step_motor->ENABLE_HARD_LIMIT=SOFT_LIMIT_CONFIG; 
						  stopStepMotor(step_motor);
							return -1;//超时
					}
					if(	step_motor->status.running == FALSE) //运行结束
					{
						step_motor->ENABLE_HARD_LIMIT=SOFT_LIMIT_CONFIG; 
							return -2;//超最大步数
					}
			 }
			 stopStepMotor(step_motor);
			 HAL_Delay(500);
			 /******************************切换运动方向慢速运行到原点位置***********************************/
			 stepperMoveWithSpeed(step_motor,speedlow,step_motor->MAX_MOVE_STEP);
			 while(HAL_GPIO_ReadPin(step_motor->SENSOR_CW_PORT,step_motor->SENSOR_CW_PIN)!=0)
			 {
					if(HAL_GetTick()-start_time>=time_out)
					{
							step_motor->ENABLE_HARD_LIMIT=SOFT_LIMIT_CONFIG; 
						  stopStepMotor(step_motor);
							return -1;//超时
					}
					if(	step_motor->status.running == FALSE) //运行结束
					{
						step_motor->ENABLE_HARD_LIMIT=SOFT_LIMIT_CONFIG; 
							return -2;//超最大步数
					}
			 }
			 stopStepMotor(step_motor);
			 setStepPosition(step_motor,0);//零点
			 step_motor->ENABLE_HARD_LIMIT=SOFT_LIMIT_CONFIG; 
			 return 1;//回零成功
				 
		}
		else  //反向回零
		{
			 /******************************快速运动到零点位置***********************************/
			 stepperMove(step_motor,-step_motor->MAX_MOVE_STEP,accel,decel,speedfast);	
			 while(HAL_GPIO_ReadPin(step_motor->SENSOR_CCW_PORT,step_motor->SENSOR_CCW_PIN)!=0)
			 {
			 		if(HAL_GetTick()-start_time>=time_out)
					{
						step_motor->ENABLE_HARD_LIMIT=SOFT_LIMIT_CONFIG; 
						stopStepMotor(step_motor);
							return -1;//超时
					}
					if(	step_motor->status.running == FALSE) //运行结束
					{
						step_motor->ENABLE_HARD_LIMIT=SOFT_LIMIT_CONFIG; 
							return -2;//超最大步数
					}
			 }
			 stopStepMotor(step_motor);
			  HAL_Delay(500);
			  /******************************切换运动方向慢速运行离开原点位置***********************************/
			 stepperMoveWithSpeed(step_motor,speedlow,step_motor->MAX_MOVE_STEP);
			 while(HAL_GPIO_ReadPin(step_motor->SENSOR_CCW_PORT,step_motor->SENSOR_CCW_PIN)==0)
			 {
					if(HAL_GetTick()-start_time>=time_out)
					{
							step_motor->ENABLE_HARD_LIMIT=SOFT_LIMIT_CONFIG; 
						  stopStepMotor(step_motor);
							return -1;//超时
					}
					if(	step_motor->status.running == FALSE) //运行结束
					{
						step_motor->ENABLE_HARD_LIMIT=SOFT_LIMIT_CONFIG; 
							return -2;//超最大步数
					}
			 }
			 stopStepMotor(step_motor);
			  HAL_Delay(500);
			 /******************************切换运动方向慢速运行到原点位置***********************************/
			 stepperMoveWithSpeed(step_motor,-speedlow,step_motor->MAX_MOVE_STEP);
			 while(HAL_GPIO_ReadPin(step_motor->SENSOR_CCW_PORT,step_motor->SENSOR_CCW_PIN)!=0)
			 {
					if(HAL_GetTick()-start_time>=time_out)
					{
							step_motor->ENABLE_HARD_LIMIT=SOFT_LIMIT_CONFIG; 
						  stopStepMotor(step_motor);
							return -1;//超时
					}
					if(	step_motor->status.running == FALSE) //运行结束
					{
						step_motor->ENABLE_HARD_LIMIT=SOFT_LIMIT_CONFIG; 
							return -2;//超最大步数
					}
			 }
			 stopStepMotor(step_motor);
			 step_motor->ENABLE_HARD_LIMIT=SOFT_LIMIT_CONFIG; 
			 setStepPosition(step_motor,0);//零点			 
			return 1;//回零成功
		}

}

/**
* @brief    设置电机运动位置
* @author   SiYu Chen
* @param[in] 
* @retval       
*/
/*! \brief 以给定的步数移动步进电机
 *  通过计算加速到最大速度，以给定的步数开始减速
 *  如果加速度和减速度很小，步进电机会移动很慢，还没达到最大速度就要开始减速
 *  \param step   移动的步数 (正数为顺时针，负数为逆时针).
 *  \param accel  加速度,如果取值为10，实际值为10*0.1*rad/sec^2=1rad/sec^2
 *  \param decel  减速度,如果取值为10，实际值为10*0.1*rad/sec^2=1rad/sec^2
 *  \param speed  最大速度,如果取值为10，实际值为10*0.1*rad/sec=1rad/sec (最小取1)；
 */

void stepperMove(step_motor_t *step_motor,int32_t step, uint32_t accel, uint32_t decel, uint32_t speed)
{  

		//--------------------------方向控制---------------------------//
		if(step == 0)
		{
			return ;
		}
		else if(step < 0)//逆时针
    {
        step_motor->srd.dir = CCW;
        step = -step; //取反
    }
    else//顺时针 大于0
    {
         step_motor->srd.dir = CW;
    }	// 输出电机方向
		HAL_GPIO_WritePin(step_motor->DIR_PORT,step_motor->DIR_PIN,(GPIO_PinState)step_motor->srd.dir); //设置方向
 
		//--------------------------决策计算---------------------------//
	
    if(step == 1)  //直接减速  
    {
        // 只移动一步
         step_motor->srd.accel_count = -1; 
        // 减速状态
         step_motor->srd.run_state = DECEL;  //直接进减速模式
        // 短延时
         step_motor->srd.step_delay = 1000;  //一个固定速度
        // 配置电机为运行状态
         step_motor->status.running = TRUE;
		
     }
		
    // 步数不为零才移动
    else if(step != 0) //如果不为0
    {
		// 设置最大速度极限, 计算得到min_delay用于定时器的计数器的值。
		// min_delay = (alpha / tt)/ w 
			
			
		 step_motor->srd.min_delay = (uint32_t)(A_T_x10/speed);  //计算出最小延时

		// 通过计算第一个(c0) 的步进延时来设定加速度，其中accel单位为0.1rad/sec^2
		// step_delay = 1/tt * sqrt(2*alpha/accel)
		// step_delay = ( tfreq*0.676/10 )*10 * sqrt( (2*alpha*100000) / (accel*10) )/100
		 //计算第一步的延时
		 step_motor->srd.step_delay = (uint32_t)((T1_FREQ_180 * sqrt(A_SQ / accel))/10);

		// 计算多少步之后达到最大速度的限制
		// max_s_lim = speed^2 / (2*alpha*accel)
		 uint32_t max_s_lim = (uint32_t)(speed*speed/(A_x200*accel/10));  //计算到达最大速度时的步数
		// 如果达到最大速度小于0.5步，我们将四舍五入为0
		// 但实际我们必须移动至少一步才能达到想要的速度
		if(max_s_lim == 0) //不能没步数
		{
				max_s_lim = 1;
		}

		// 计算多少步之后我们必须开始减速
		// n1 = (n1+n2)decel / (accel + decel)
		 float ratio = decel/(float)(accel+decel);
		
		 uint32_t accel_lim = (uint32_t)(ratio*step); //减速在的步长
		// 我们必须加速至少1步才能才能开始减速.
		if(accel_lim == 0) //加速至少一部
		{
			accel_lim = 1;
		}
		// 使用限制条件我们可以计算出第一次开始减速的位置
		//srd.decel_val为负数
		if(accel_lim <= max_s_lim) //如果不能达到最大速度
		{
			 step_motor->srd.decel_val = (accel_lim - step);
		}
		else{
			 step_motor->srd.decel_val = -(max_s_lim*accel/decel);
		}
		// 当只剩下一步我们必须减速
		if( step_motor->srd.decel_val == 0)
		{
			 step_motor->srd.decel_val = -1;
		}

		// 计算开始减速时的步数
		 step_motor->srd.decel_start = (uint32_t)(step +  step_motor->srd.decel_val);

		// 如果计算的第一步速度太大
		if( step_motor->srd.step_delay <=  step_motor->srd.min_delay)
		{
			 step_motor->srd.step_delay =  step_motor->srd.min_delay; //限制速度
			 step_motor->srd.run_state = RUN;
		}
		else //否则就慢慢加速
		{
			 step_motor->srd.run_state = ACCEL;
		}
		// 复位加速度计数值
		 step_motor->srd.accel_count = 0;
		 step_motor->status.running = TRUE;
	}

		uint16_t tim_count=__HAL_TIM_GET_COUNTER(step_motor->TIMx);
	  uint32_t nex_cnt = (uint32_t)(step_motor->srd.step_delay/2);
	
		if(nex_cnt>=65535)
		{
				nex_cnt = 65535;
		}
		uint16_t ccr = (tim_count+nex_cnt);
		

		/*在当前计数值基础上设置定时器比较值*/
		__HAL_TIM_SET_COMPARE(step_motor->TIMx,step_motor->channel,ccr); 
		
		/*使能定时器通道*/
		TIM_CCxChannelCmd(step_motor->TIMx->Instance, step_motor->channel, TIM_CCx_DISABLE); //使能比较通道
		__HAL_TIM_ENABLE_IT(step_motor->TIMx,step_motor->IT_channel);
		__HAL_TIM_ENABLE(step_motor->TIMx);  //定时器使能
}


/**
* @brief    直接定速运动没有加减速过程
* @author   SiYu Chen
* @param[in] speed  最大速度,如果取值为10，实际值为10*0.1*rad/sec=1rad/sec
* @param[in] max_step  最大的移动步数 范围（0~4294967295）
						 应该设置为不超过最大行程加1，这样可以防止传感器失效，一直转动导致的结构损坏
 */

void stepperMoveWithSpeed(step_motor_t *step_motor,int32_t speed,uint32_t max_step)
{  
	
		if(max_step == 0)
		{
			return ;
		}
	
		//--------------------------方向控制---------------------------//
		if(speed == 0)
		{
			stopStepMotor(step_motor);//如果速度为0，直接停电机
			return;
		}
		else if(speed < 0)//逆时针
    {
        step_motor->srd.dir = CCW;
        speed = -speed; //取反
    }
    else//顺时针 大于0
    {
         step_motor->srd.dir = CW;
    }	// 输出电机方向
		HAL_GPIO_WritePin(step_motor->DIR_PORT,step_motor->DIR_PIN,(GPIO_PinState)step_motor->srd.dir); //设置方向
 
		
    if(max_step == 1)  //直接减速  
    {
        // 只移动一步
         step_motor->srd.accel_count = -1; 
        // 减速状态
         step_motor->srd.run_state = DECEL;  //直接进减速模式
        // 短延时
         step_motor->srd.step_delay = 1000;  //一个固定速度
        // 配置电机为运行状态
         step_motor->status.running = TRUE;
		
     }
		
    // 步数不为零才移动
    else if(max_step != 0) //如果不为0
    {
		
		//--------------------------决策计算---------------------------//

		step_motor->srd.min_delay = (uint32_t)(A_T_x10/speed);  //计算达到期望速度的最小延时
		
		step_motor->srd.decel_val = -1; //最后一步必须减
		
    step_motor->srd.decel_start = (uint32_t)(max_step +  step_motor->srd.decel_val); //计算开始减速的位置
		
		step_motor->srd.step_delay =  step_motor->srd.min_delay; //以最大速度直接跑
		
		step_motor->srd.accel_count = 0; 	// 复位加速度计数值

		step_motor->srd.run_state = RUN;  
		step_motor->status.running = TRUE;
		
		}
		uint16_t tim_count=__HAL_TIM_GET_COUNTER(step_motor->TIMx);
	  uint32_t nex_cnt = (uint32_t)(step_motor->srd.step_delay/2);
	
		if(nex_cnt>=65535)
		{
				nex_cnt = 65535;
		}
		uint16_t ccr = (tim_count+nex_cnt);
		/*在当前计数值基础上设置定时器比较值*/
		__HAL_TIM_SET_COMPARE(step_motor->TIMx,step_motor->channel,ccr); 
		
	
		/*使能定时器通道*/
		TIM_CCxChannelCmd(step_motor->TIMx->Instance, step_motor->channel, TIM_CCx_DISABLE); //使能比较通道
		__HAL_TIM_ENABLE_IT(step_motor->TIMx,step_motor->IT_channel);

		__HAL_TIM_ENABLE(step_motor->TIMx);  //定时器使能
		
}

/**
* @brief    定速运动有加速过程
* @author   SiYu Chen
* @param[in] accel  加速度,如果取值为10，实际值为10*0.1*rad/sec^2=1rad/sec^2
* @param[in] speed  最大速度,如果取值为10，实际值为10*0.1*rad/sec=1rad/sec
* @param[in] max_step  最大的移动步数 范围（0~4294967295）
						 应该设置为不超过最大行程加1，这样可以防止传感器失效，一直转动导致的结构损坏
 */
void stepperMoveWithAcc(step_motor_t *step_motor,uint32_t accel,int32_t speed,uint32_t max_step)
{  
	
			if(max_step == 0)
		{
			return ;
		}
	
		//--------------------------方向控制---------------------------//
		if(speed == 0)
		{
			stopStepMotor(step_motor);//如果速度为0，直接停电机
			return ;
		}
		else if(speed < 0)//逆时针
    {
        step_motor->srd.dir = CCW;
        speed = -speed; //取反
    }
    else//顺时针 大于0
    {
         step_motor->srd.dir = CW;
    }	// 输出电机方向
		HAL_GPIO_WritePin(step_motor->DIR_PORT,step_motor->DIR_PIN,(GPIO_PinState)step_motor->srd.dir); //设置方向
 
		
		
			
    if(max_step == 1)  //直接减速  
    {
        // 只移动一步
         step_motor->srd.accel_count = -1; 
        // 减速状态
         step_motor->srd.run_state = DECEL;  //直接进减速模式
        // 短延时
         step_motor->srd.step_delay = 1000;  //一个固定速度
        // 配置电机为运行状态
         step_motor->status.running = TRUE;
		
     }
		
    // 步数不为零才移动
    else if(max_step != 0) //如果不为0
    {
		
		//--------------------------决策计算---------------------------//
		step_motor->srd.run_state = ACCEL;  
		step_motor->status.running = TRUE;
		
		step_motor->srd.min_delay = (uint32_t)(A_T_x10/speed);  //计算达到期望速度的最小延时
		
		step_motor->srd.decel_val = -1; //最后一步必须减
		
			
    step_motor->srd.decel_start = (uint32_t)(max_step +  step_motor->srd.decel_val); //计算开始减速的位置
		
	
		step_motor->srd.accel_count = 0; 	// 复位加速度计数值

		 //计算第一步的速度
		step_motor->srd.step_delay = (uint32_t)((T1_FREQ_180 * sqrt(A_SQ / accel))/10);

		if( step_motor->srd.step_delay <=  step_motor->srd.min_delay) //如果加速度太大
		{
			 step_motor->srd.step_delay =  step_motor->srd.min_delay; //以最大速度直接跑
		}
		
		// 计算多少步之后达到最大速度的限制
		// max_s_lim = speed^2 / (2*alpha*accel)  
		uint32_t max_s_lim = (uint32_t)(speed*speed/(A_x200*accel/10));  //计算到达最大速度时的步数
			
		}
		
		
		uint16_t tim_count=__HAL_TIM_GET_COUNTER(step_motor->TIMx);
	  uint32_t nex_cnt = (uint32_t)(step_motor->srd.step_delay/2);
	
		if(nex_cnt>=65535)
		{
				nex_cnt = 65535;
		}
		uint16_t ccr = (tim_count+nex_cnt);
		/*在当前计数值基础上设置定时器比较值*/
		__HAL_TIM_SET_COMPARE(step_motor->TIMx,step_motor->channel,ccr); 
		
		
	
		/*使能定时器通道*/
		TIM_CCxChannelCmd(step_motor->TIMx->Instance, step_motor->channel, TIM_CCx_DISABLE); //使能比较通道
		__HAL_TIM_ENABLE_IT(step_motor->TIMx,step_motor->IT_channel);

		__HAL_TIM_ENABLE(step_motor->TIMx);  //定时器使能
		
}



/**
* @brief    获取当前位置脉冲
* @author   SiYu Chen
* @param[in] 
* @retval       
*/

int32_t getStepPosition(step_motor_t *step_motor)
{  
		return step_motor->step_position;
}


/**
* @brief    获取当前绝对位置单位cm
* @author   SiYu Chen
* @param[in] 
* @retval       
*/

float getAbsPosition(step_motor_t *step_motor)
{  
		float distance =  step_motor->step_position/SPR*step_motor->screw_pitch_cm;
		return distance;
}


/**
* @brief    设置当前记录位置
* @author   SiYu Chen
* @param[in] 
* @retval       
*/
void setStepPosition(step_motor_t *step_motor,int32_t position)
{  
		step_motor->step_position=position;
}

/**
* @brief    停止电机
* @author   SiYu Chen
* @param[in] 
* @retval       
*/
void stopStepMotor(step_motor_t *step_motor)
{  
		step_motor->srd.run_state = STOP;
		step_motor->srd.step_count = 0;  // 清零步数计数器
		step_motor->srd.rest = 0;        // 清零余值
		// 关闭通道
		step_motor->status.running = FALSE;
		step_motor->srd.accel_count=0;
		step_motor->srd.decel_start=0;
		step_motor->srd.decel_val=0;
		step_motor->srd.min_delay=0;
		step_motor->srd.step_delay=0;
		// 加速过程中最后一次延时（脉冲周期）.
		step_motor->srd.last_accel_delay=0;
		TIM_CCxChannelCmd(step_motor->TIMx->Instance, step_motor->channel, TIM_CCx_DISABLE); //关闭信号输出
		__HAL_TIM_DISABLE_IT(step_motor->TIMx, step_motor->IT_channel);	//关闭中断源
		HAL_GPIO_WritePin(step_motor->PUL_PORT,step_motor->PUL_PIN,0);
}

/**
* @brief    使能或者失能电机
* @author   SiYu Chen
* @param[in] 
* @retval       
*/
void enableStepMotor(step_motor_t *step_motor,_Bool enable)
{  
		if(enable)
		{
			HAL_GPIO_WritePin(step_motor->ENA_PORT,step_motor->ENA_PIN,STEP_ENABLE); 
		}
		else
		{
			HAL_GPIO_WritePin(step_motor->ENA_PORT,step_motor->ENA_PIN,STEP_DISENABLE); 
			stopStepMotor(step_motor);
		}
}



/**
* @brief    返回电机是否到位 1到位 0没有到位
* @author   SiYu Chen
* @param[in] 
* @retval       
*/
_Bool checkStepMotorFinish(step_motor_t *step_motor)
{
		if(step_motor->status.running==FALSE)
		{
			return 1;
		}
		else
		{
			return 0;
		}
}

/**
  * @brief  电机转动指定圈数
	*	@note 	
  * @retval 无
  */
void	motorMoveWithCircle(step_motor_t *step_motor,int32_t circle, uint32_t accel, uint32_t decel, uint32_t speed)
{
	int32_t step;
	step = circle * 100000;
	stepperMove(step_motor,step,accel,decel,speed);
}



/**
  * @brief  电机转动指定位置
	*	@note 	
  * @retval 无
  */

void motorMoveWithPosition(step_motor_t *step_motor,int32_t target_position, uint32_t accel, uint32_t decel, uint32_t speed)
{
	int32_t step = target_position - (step_motor->step_position);
	stepperMove(step_motor,step,accel,decel,speed);
}


/**
  * @brief  速度决策
	*	@note 	在中断中使用，每进一次中断，决策一次
  * @retval 无
  */

void speed_decision(step_motor_t *step_motor)
{

		
		// 保存新（下）一个延时周期
		uint32_t new_step_delay=0;
		// 设置比较值
		uint16_t tim_count=__HAL_TIM_GET_COUNTER(step_motor->TIMx);
	  uint32_t nex_cnt = (uint32_t)(step_motor->srd.step_delay/2);
		if(nex_cnt>=65535)
		{
				nex_cnt = 65535;
		}
		uint16_t ccr = (tim_count+nex_cnt);
		/*在当前计数值基础上设置定时器比较值*/
		__HAL_TIM_SET_COMPARE(step_motor->TIMx,step_motor->channel,ccr); 

		step_motor->srd.i++;     // 定时器中断次数计数值
		if(step_motor->ENABLE_OUT==1) //使能输出 
		{
				HAL_GPIO_TogglePin(step_motor->PUL_PORT,step_motor->PUL_PIN);	
		}

		if(step_motor->srd.i==2) // 2次，说明已经输出一个完整脉冲
		{
			step_motor->srd.i=0;   // 清零定时器中断次数计数值
			switch(step_motor->srd.run_state) 
			{
				/*步进电机停止状态*/
				case STOP:
				step_motor->srd.step_count = 0;  // 清零步数计数器
				step_motor->srd.rest = 0;        // 清零余值
				// 关闭通道
				HAL_GPIO_WritePin(step_motor->PUL_PORT,step_motor->PUL_PIN,0);
				__HAL_TIM_DISABLE_IT(step_motor->TIMx, step_motor->IT_channel);	//关闭中断源
				TIM_CCxChannelCmd(step_motor->TIMx->Instance, step_motor->channel, TIM_CCx_DISABLE); //关闭信号输出
				step_motor->status.running = FALSE;
				step_motor->ENABLE_OUT = 0;
				break;
				/*步进电机加速状态*/
				case ACCEL:		
				/*监测限位开关*/
				if(step_motor->ENABLE_HARD_LIMIT&&step_motor->srd.dir==CW && HAL_GPIO_ReadPin(step_motor->SENSOR_CW_PORT,step_motor->SENSOR_CW_PIN)==0)
				{
						stopStepMotor(step_motor);
						return;				
				}
				else if(step_motor->ENABLE_HARD_LIMIT&&step_motor->srd.dir==CCW && HAL_GPIO_ReadPin(step_motor->SENSOR_CCW_PORT,step_motor->SENSOR_CCW_PIN)==0)
				{
						stopStepMotor(step_motor);
						return;
				}
				TIM_CCxChannelCmd(step_motor->TIMx->Instance, step_motor->channel, TIM_CCx_ENABLE);
				step_motor->ENABLE_OUT = 1;
				step_motor->srd.step_count++;
				step_motor->srd.accel_count++;
				if(step_motor->srd.dir==CW)  //记录运动步数
				{
						step_motor->step_position+=1;
				}
				else
				{
						step_motor->step_position-=1;
				}

				new_step_delay = step_motor->srd.step_delay - (((2 *step_motor->srd.step_delay) + step_motor->srd.rest)/(4 * step_motor->srd.accel_count + 1));//计算新(下)一步脉冲周期(时间间隔)
				
		
				
				
				step_motor->srd.rest = ((2 * step_motor->srd.step_delay)+step_motor->srd.rest)%(4 * step_motor->srd.accel_count + 1);// 计算余数，下次计算补上余数，减少误差
				//检查是够应该开始减速
					if(step_motor->srd.step_count >= step_motor->srd.decel_start) {
						step_motor->srd.accel_count = step_motor->srd.decel_val;
						step_motor->srd.run_state = DECEL;
					}
					//检查是否到达期望的最大速度
					else if(new_step_delay <= step_motor->srd.min_delay) 
				 {
						step_motor->srd.last_accel_delay = new_step_delay;
						new_step_delay = step_motor->srd.min_delay;    
						step_motor->srd.rest = 0;                          
						step_motor->srd.run_state = RUN;
					}
					break;
				/*步进电机最大速度运行状态*/
					case RUN:
					/*监测限位开关*/
					if(step_motor->ENABLE_HARD_LIMIT&&step_motor->srd.dir==CW && HAL_GPIO_ReadPin(step_motor->SENSOR_CW_PORT,step_motor->SENSOR_CW_PIN)==0)
					{
							stopStepMotor(step_motor);
							return;				
					}
					else if(step_motor->ENABLE_HARD_LIMIT&&step_motor->srd.dir==CCW && HAL_GPIO_ReadPin(step_motor->SENSOR_CCW_PORT,step_motor->SENSOR_CCW_PIN)==0)
					{
							stopStepMotor(step_motor);
							return;
					}		
					step_motor->ENABLE_OUT = 1;
					TIM_CCxChannelCmd(step_motor->TIMx->Instance, step_motor->channel, TIM_CCx_ENABLE);
					step_motor->srd.step_count++;
					if(step_motor->srd.dir==CW)  //记录运动步数
					{
						step_motor->step_position+=1;
					}
					else
					{
						step_motor->step_position-=1;
					}

					new_step_delay = step_motor->srd.min_delay;
					//检查是否需要开始减速
					if(step_motor->srd.step_count >= step_motor->srd.decel_start) 
					{
						step_motor->srd.accel_count = step_motor->srd.decel_val;
						//以最后一次加速的延时作为开始减速的延时
						new_step_delay = step_motor->srd.last_accel_delay;
						step_motor->srd.run_state = DECEL;	
					}
					break;
				/*步进电机减速状态*/
				case DECEL:
					
					/*监测限位开关*/
					if(step_motor->ENABLE_HARD_LIMIT&&step_motor->srd.dir==CW && HAL_GPIO_ReadPin(step_motor->SENSOR_CW_PORT,step_motor->SENSOR_CW_PIN)==0)
					{
							stopStepMotor(step_motor);
							return;				
					}
					else if(step_motor->ENABLE_HARD_LIMIT&&step_motor->srd.dir==CCW && HAL_GPIO_ReadPin(step_motor->SENSOR_CCW_PORT,step_motor->SENSOR_CCW_PIN)==0)
					{
							stopStepMotor(step_motor);
							return;
					}
					step_motor->ENABLE_OUT = 1;
					TIM_CCxChannelCmd(step_motor->TIMx->Instance, step_motor->channel, TIM_CCx_ENABLE);
	
					step_motor->srd.step_count++;
					step_motor->srd.accel_count++;
					if(step_motor->srd.dir==CW)  //记录运动步数
					{
							step_motor->step_position+=1;
					}
					else
					{
							step_motor->step_position-=1;
					}
					new_step_delay = step_motor->srd.step_delay - (((2 * step_motor->srd.step_delay) + step_motor->srd.rest)/(4 * step_motor->srd.accel_count + 1)); //计算新(下)一步脉冲周期(时间间隔)
					step_motor->srd.rest = ((2 * step_motor->srd.step_delay)+step_motor->srd.rest)%(4 * step_motor->srd.accel_count + 1);// 计算余数，下次计算补上余数，减少误差

					//检查是否为最后一步
					if(step_motor->srd.accel_count >= 0)
					{
						step_motor->srd.run_state = STOP;
 
					}
					break;
			}
			/*求得下一次间隔时间*/
			step_motor->srd.step_delay = new_step_delay;
		}
}



/**
  * @brief  电机以国际单位增量式运动
	*	@note
	* @param[in] distance_cm  运动距离  最大±2147483.647cm
	* @param[in] acc_cm  加速度,cm/sec^2
	* @param[in] decr_cm  减速度,cm/sec^2
	* @param[in] speed  最大速度 cm/sec^2 最大954cm/s

  * @retval 无
  */
void stepperMoveDelta_CM (step_motor_t *step_motor,float distance_cm, float acc_cm, uint32_t decr_cm, float speed)
{
	
	 int32_t step = floorf( distance_cm*SPR/step_motor->screw_pitch_cm);
   uint32_t move_speed = floorf(speed/step_motor->screw_pitch_cm*2*3.141592653f*10);
	 if(move_speed<=1)
	 {
			move_speed=1;
	 }
	 
	  uint32_t accel = floorf(acc_cm/step_motor->screw_pitch_cm*2*3.141592653f*10);
	 if(accel<=1)
	 {
			accel=1;
	 }
	 
	 uint32_t decel = floorf(decr_cm/step_motor->screw_pitch_cm*2*3.141592653f*10);
	 if(decel<=1)
	 {
			decel=1;
	 }
	  stepperMove(step_motor,step,  accel,  decel,  move_speed);
}


/**
  * @brief  电机以国际单位绝对式运动
	*	@note
	* @param[in] distance_cm  运动距离  最大±2147483.647cm
	* @param[in] acc_cm  加速度,cm/sec^2
	* @param[in] decr_cm  减速度,cm/sec^2
	* @param[in] speed  最大速度 cm/sec^2 最大954cm/s

  * @retval 无
  */
void stepperMoveAbs_CM (step_motor_t *step_motor,float abs_distance_cm, float acc_cm, uint32_t decr_cm, float speed)
{
	
	 int32_t step = floorf( abs_distance_cm*SPR/step_motor->screw_pitch_cm);
   uint32_t move_speed = floorf(speed/step_motor->screw_pitch_cm*2*3.141592653f*10);
	 if(move_speed<=1)
	 {
			move_speed=1;
	 }
	 
	  uint32_t accel = floorf(acc_cm/step_motor->screw_pitch_cm*2*3.141592653f*10);
	 if(accel<=1)
	 {
			accel=1;
	 }
	 
	 uint32_t decel = floorf(decr_cm/step_motor->screw_pitch_cm*2*3.141592653f*10);
	 if(decel<=1)
	 {
			decel=1;
	 }
	 
	 int32_t out =  step - step_motor->step_position;

	 stepperMove(step_motor,out,  accel,  decel,  move_speed);
}







